#include "flash.h"
union u32_u8{
	u32 word;
	u8 ch[4];
};
union u32_u8 Data;

u32 F460FLASH_ReadWord(u32 faddr){
	return *(vu32*)faddr; 
}

void EFM_Program(u32 WriteAddr,u16 NumToWrite,u8 *pBuffer){
	u16 i=0;
	u32 temp=0;
	for(i=0;i<NumToWrite/4;i++){
		Data.ch[0]=pBuffer[i*4];
		Data.ch[1]=pBuffer[i*4+1];
		Data.ch[2]=pBuffer[i*4+2];
		Data.ch[3]=pBuffer[i*4+3];
		EFM_SingleProgram(WriteAddr,Data.word);
		WriteAddr+=4;
	}
	if(NumToWrite%4==1){
		temp=pBuffer[i*4]|0xFFFFFF00;
		EFM_SingleProgram(WriteAddr,temp);
	}else if(NumToWrite%4==2){
		temp=pBuffer[i*4]|(u32)(pBuffer[i*4+1])<<8|0xFFFF0000;
		EFM_SingleProgram(WriteAddr,temp);
	}else if(NumToWrite%4==3){
		temp=pBuffer[i*4]|(u32)(pBuffer[i*4+1])<<8|(u32)(pBuffer[i*4+2])<<16|0xFF000000;
		EFM_SingleProgram(WriteAddr,temp);
	}
}

u8 F460FLASH_BUF[F460_SECTOR_SIZE];
void F460FLASH_Write(u32 WriteAddr,u8 *pBuffer,u16 NumToWrite){
	u32 sectorpos;
	u16 sectoroff;
	u16 sectorremain;   
 	u16 i;    
	u32 offaddr;
	if(WriteAddr>=(F460_FLASH_BASE+1024*F460_FLASH_SIZE)){
		return;
	}
	EFM_Unlock();
	EFM_FlashCmd(Enable);
	while(Set != EFM_GetFlagStatus(EFM_FLAG_RDY)){;}
	offaddr=WriteAddr-F460_FLASH_BASE;
	sectorpos=offaddr/F460_SECTOR_SIZE; 
	sectoroff=(offaddr%F460_SECTOR_SIZE)/4;
	sectorremain=F460_SECTOR_SIZE/4-sectoroff;  
	if(NumToWrite<=sectorremain){
		sectorremain=NumToWrite;
	}
	while(1) {	
		F460FLASH_Read(sectorpos*F460_SECTOR_SIZE+F460_FLASH_BASE,F460FLASH_BUF,F460_SECTOR_SIZE);
		for(i=0;i<sectorremain*4;i++){
			if(F460FLASH_BUF[sectoroff*4+i]!=0xFF){
				break;
			}				
		}
		if(i<sectorremain){
			EFM_SectorErase(sectorpos*F460_SECTOR_SIZE);
			for(i=0;i<sectorremain*4;i++){
				F460FLASH_BUF[i+sectoroff*4]=pBuffer[i];	  
			}
			EFM_Program(sectorpos*F460_SECTOR_SIZE, F460_SECTOR_SIZE, F460FLASH_BUF);
		}else {
			EFM_Program(WriteAddr,sectorremain*4, pBuffer); 
		}
		if(NumToWrite==sectorremain){
			break;
		}else{
			sectorpos++;
			sectoroff=0;	 
			pBuffer+=sectorremain;
			WriteAddr+=sectorremain*4;  
			NumToWrite-=sectorremain;
			if(NumToWrite>(F460_SECTOR_SIZE/4)){
				sectorremain=F460_SECTOR_SIZE/4;
			}else{
				sectorremain=NumToWrite;
			}
		}	
	};	
	EFM_Lock();
}

void F460FLASH_Sector_Erase(u32 WriteAddr,u32 num){	
	u32 cnt=0;
	if(WriteAddr>=(F460_FLASH_BASE+1024*F460_FLASH_SIZE)){
		return;
	}
	EFM_Unlock();
	EFM_FlashCmd(Enable);
	while(Set != EFM_GetFlagStatus(EFM_FLAG_RDY)){;}
	while(1) {	
		EFM_SectorErase(WriteAddr);
		WriteAddr+=8192;
		cnt+=8192;
		if(cnt>num){
			break;
		}
	}	
	EFM_Lock();
}

void F460FLASH_Write1(u32 WriteAddr,u8 *pBuffer,u16 NumToWrite){
	u32 sectorpos;
	u16 sectoroff;
	u16 sectorremain; 
 	u16 i;    
	u32 offaddr;
	if(WriteAddr>=(F460_FLASH_BASE+1024*F460_FLASH_SIZE)){
		return;
	}
	EFM_Unlock();
	EFM_FlashCmd(Enable);
	while(Set != EFM_GetFlagStatus(EFM_FLAG_RDY)){;}	
	EFM_Program(WriteAddr,NumToWrite, pBuffer);
	EFM_Lock();
}

void F460FLASH_Read(u32 ReadAddr,u8 *pBuffer,u16 NumToRead){
	u16 i;
	u32 temp;
	for(i=0;i<NumToRead/4;i++){
		Data.word=F460FLASH_ReadWord(ReadAddr);
		ReadAddr+=4;
		pBuffer[i*4]=Data.ch[0];
		pBuffer[i*4+1]=Data.ch[1];
		pBuffer[i*4+2]=Data.ch[2];
		pBuffer[i*4+3]=Data.ch[3];
	}
}
